from naoqi import ALProxy
from walkToObstacle import walkToObstacleTOWARDversion
from walkToObstacle import walkToEnd
from obstacle import obstacle


def launch(robotIP, PORT=9559):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    angleINTI = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    walkToObstacleTOWARDversion(robotIP, PORT, angleINTI, "blue")
    obstacle(robotIP, PORT, angleINTI, 1)
    walkToEnd(robotIP, PORT, angleINTI)
    motionProxy.rest()


if __name__ == '__main__':
    robotIP = "169.254.134.149"
    PORT = 9559
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    launch(robotIP, PORT)
    # while True:
    #     print "\nangleZ: "
    #     print valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print "angleX: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value")
    # print "angleY "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value")
    #
    # print "\nGyroscopeZ: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/GyroscopeZ/Sensor/Value")
    # print "GyroscopeX: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/GyroscopeX/Sensor/Value")
    # print "GyroscopeY: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/GyroscopeY/Sensor/Value")
    # print "\nAccelerometerZ: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/AccelerometerZ/Sensor/Value")
    # print "AccelerometerX: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/AccelerometerX/Sensor/Value")
    # print "AccelerometerY: "
    # print valueProxy.getData("Device/SubDeviceList/InertialSensor/AccelerometerY/Sensor/Value")
